classdef Path < handle
    properties (Access = public)
        Trajectory(1,200) = Point();
        Traject_Size = 0;
    end
    methods (Access = public)
        function obj = Path(size)
            if size > 200 || size <= 1
                obj.Traject_Size = 200;
                return;
            end
            obj.Traject_Size = size;
            for i =1:100
                obj.Trajectory(1,i).x = 0.0;
                obj.Trajectory(1,i).y = 0.0;
                obj.Trajectory(1,i).yaw = 0.0;
                obj.Trajectory(1,i).yaw_rate = 0.0;
                obj.Trajectory(1,i).velocity = 0.0;
            end 
        end
        
        function Generating_Cycle_Path(obj,start_ponit,turn_angle,gear_mode,turn_rate)
            x_f = start_ponit.x;
            y_f = start_ponit.y;
            yaw_f = start_ponit.yaw;
            ga_angle = turn_angle;
            mode = gear_mode;
            Generating_Cycle_ARR = ARR_Kinematic_Model(x_f,y_f,yaw_f,ga_angle,mode);
            obj.Trajectory(1) = Generating_Cycle_ARR.P_f;
            sw_enable = 1;
            for i = 2 : obj.Traject_Size
                if mod(i,100) == 0
                    sw_enable = -1*sw_enable;
                end
%                 fprintf('i %d sw_enable %d \n',i,sw_enable);
                if sw_enable == 1
                    ga_angle_rate = turn_rate;
                else 
%                     Generating_Cycle_ARR.steer_wheel_angle = 0;
                    ga_angle_rate = -turn_rate;
                end
                Generating_Cycle_ARR.State_Update(5, 0.1, ga_angle_rate);
                obj.Trajectory(i) = Generating_Cycle_ARR.P_f;
            end
        end
        
        function Index = Find_Closest_Point(obj,s_index,f_point,r_point,g_mode)
            index = obj.Traject_Size;
            if s_index >= obj.Traject_Size
                fprintf('err sindex %d',s_index);
                return;
            end
            
            for i = s_index:obj.Traject_Size
                if g_mode == 1
                    theta = atan((obj.Trajectory(i).y - r_point.y)/(obj.Trajectory(i).x - r_point.x));
                    if (obj.Trajectory(i).y - r_point.y) > 0 && (obj.Trajectory(i).x - r_point.x) < 0
                        theta = theta + pi;
                    end
                    if (obj.Trajectory(i).y - r_point.y) < 0 && (obj.Trajectory(i).x - r_point.x) < 0
                        theta = theta - pi;
                    end
                    x_trans = (f_point.x - obj.Trajectory(i).x)*cos(theta) + (f_point.y - obj.Trajectory(i).y)*sin(theta);
                else
                    theta = atan((obj.Trajectory(i).y - f_point.y)/(obj.Trajectory(i).x - f_point.x));
                    if (obj.Trajectory(i).y - f_point.y) > 0 && (obj.Trajectory(i).x - f_point.x) < 0
                        theta = theta + pi;
                    end
                    if (obj.Trajectory(i).y - f_point.y) < 0 && (obj.Trajectory(i).x - f_point.x) < 0
                        theta = theta - pi;
                    end
                    x_trans = (r_point.x - obj.Trajectory(i).x)*cos(theta) + (r_point.y - obj.Trajectory(i).y)*sin(theta);
                end
                
                
                fprintf('index %d theta %f xtrans %f\n',i,180*theta/pi,x_trans);
                if x_trans < -2
                    index = i;
                    break;
                end
                
            end
            
            Index = index;
        end
    end
end